/*
 * ImageDeal.c
 *
 *  Created on: 2017年7月12日
 *      Author: changruowang
 */

#include "system.h"
#include "user_bsp.h"
#include <math.h>

Point_Status_St Point_Status_E;
Feature_Point_St Feature_Point_E=N0_Right_Angle;
Point_Num_St  Point_Num_E;
Point_St Point_t;


YesNo_t EN_SendMsgToFmu=No;
YesNo_t EN_PID_Calcu=No;
YesNo_t EN_Offest_Rotate=No;

uint8_t Image_Bin[CAMERA_Fit_H][CAMERA_Fit_W] = {0};

YesNo_t Have_front_Point;
YesNo_t Have_back_Point;
YesNo_t	Have_left_Point;
YesNo_t Have_right_Point;

Fly_Status_St Fly_Status_t;
uint8_t Image_thr=60;
//u8 ImagRecOK = 0;


/*
void Image_Deal()
{

   Find_Edge_Point();
 //  Image_Correct();
//  SCI1_Printf("%d %d %d\r\n",Point_Num_E,Point_Status_E,Mode_Two_St_Act);
}
*/


void ParaInit(void)
{
	Position_Enum i;

	Point_Num_E=Point_Zero;
	Point_Status_E=Point_Status_Error;

	Have_front_Point=No;
	Have_back_Point=No;
	Have_left_Point=No;
	Have_right_Point=No;

//	Point_t.mid_x=0;
//	Point_t.mid_y=0;
    Point_t.Rotate_Offest=0;
	EN_Offest_Rotate=No;
	Feature_Point_E=N0_Right_Angle;
	for(i=front;i<=back;i++)
	{
		Point_t.Locate[i].x=0;
		Point_t.Locate[i].y=0;
		Point_t.Locate[i].x_begin=0;
		Point_t.Locate[i].x_end=CAMERA_Fit_W-1;
		Point_t.Locate[i].y_begin=0;
		Point_t.Locate[i].y_end=0;
		Point_t.Locate[i].point_cnt=0;
	}

	for(i=left;i<=right;i++)
	{
		Point_t.Locate[i].x=0;
		Point_t.Locate[i].y=0;
		Point_t.Locate[i].x_begin=0;
		Point_t.Locate[i].x_end=0;
		Point_t.Locate[i].y_begin=0;
		Point_t.Locate[i].y_end=CAMERA_Fit_H-1;
		Point_t.Locate[i].point_cnt=0;
	}
}

void Image_Correct()
{
		s8 temp1=0,temp2=0;
		temp1=(s8)(Fly_Status_t.Height*my_sin(AngToRad(Fly_Status_t.ROLL))*Camera_ROLL/Fly_Status_t.Height);
		if(Point_t.mid_x>temp1)
			Point_t.mid_x-=temp1;
		else Point_t.mid_x=0;

		temp2=(s8)(Fly_Status_t.Height*my_sin(AngToRad(Fly_Status_t.PIT))*Camera_PIT/Fly_Status_t.Height);
		if(Point_t.mid_y>temp2)
			Point_t.mid_y-=temp2;
		else Point_t.mid_y=0;

		Point_t.mid_x=LIMIT(Point_t.mid_x,1,80);
		Point_t.mid_y=LIMIT(Point_t.mid_y,1,80);
}

//找边缘点
void Find_Edge_Point()
{
	ParaInit();

	Search_Line_Point(1,front);


	Search_Line_Point(CAMERA_Fit_H-2,back);


	Search_Column_Point(2,left);


	Search_Column_Point(CAMERA_Fit_W-2,right);

	Find_Point_Position();
	Fine_Offest_Rotate();

}

void Find_Point_Position()
{
	switch(Point_Num_E)
	{
		case Point_Error:
		{

		}
		break;
		case Point_Zero:
		{

		}
		break;

		case Point_One:
		{

		}
		break;

		case Point_Two:
		{
			Deal_Two_Point();
		}
		break;

		case Point_Three:
		{
			Deal_Three_Point();
		}
		break;

		case Point_Four:
		{
			Deal_Four_Point();
		}
		break;
	}
}
void	Fine_Offest_Rotate()
{
		#define NUMM ((CAMERA_Fit_H/2)+5)
//		uint8_t STA_Pos;
//		uint8_t END_Pos;
		uint8_t i;
		uint8_t Buff_Begin[NUMM]={0};
		uint8_t Buff_End[NUMM]={0};
		uint8_t Find_OK=0;
		s16 sum=0;


		if(EN_Offest_Rotate==No)
		{
			Point_t.Rotate_Offest=0;
			return;
		}
		switch(Point_Status_E)
		{
			case Two_Top_Bottom:
			{
				u8 STA_Pos=2;
				u8 END_Pos=CAMERA_Fit_H/2;
				for(i=0;i<(END_Pos-STA_Pos);i++)
				{
					Buff_Begin[i]=CAMERA_Fit_W-1;
					if(Find_OK==0)
					{
							Find_OK=Scan_One_Line(i+STA_Pos,Buff_Begin,Buff_End,i,front);
					}
					else if(Find_OK==1)
					{
						Find_OK=Edge_Track_Line((Buff_Begin[i-1]+Buff_End[i-1])/2,STA_Pos+i,Buff_Begin,Buff_End,i,front);
					}
				}
				for(i=0;i<(END_Pos-STA_Pos);i++)
				{
					sum+=((Buff_End[END_Pos-STA_Pos-1]+Buff_Begin[END_Pos-STA_Pos-1])-(Buff_End[i]+Buff_Begin[i]))/2;
				}
				Point_t.Rotate_Offest=sum;
			}
			break;

			case Two_Left_Right:
			{
				u8 STA_Pos=2;
				u8 END_Pos=CAMERA_Fit_H/2;

				for(i=0;i<(END_Pos-STA_Pos);i++)
				{
					Buff_End[i]=CAMERA_Fit_H-1;
					if(Find_OK==0)
					{
							Find_OK=Scan_One_Column(i+STA_Pos,Buff_Begin,Buff_End,i,left);
					}
					else if(Find_OK==1)
					{
						Find_OK=Edge_Track_Column((Buff_Begin[i-1]+Buff_End[i-1])/2,STA_Pos+i,Buff_Begin,Buff_End,i,left);
					}
				}
				for(i=0;i<(END_Pos-STA_Pos);i++)
				{
					sum+=((Buff_End[END_Pos-STA_Pos-1]+Buff_Begin[END_Pos-STA_Pos-1])-(Buff_End[i]+Buff_Begin[i]))/2;
				}
				Point_t.Rotate_Offest=-sum;
			}
			break;
		}

}




void Deal_Four_Point()
{
	if(Have_front_Point==Yes&&Have_back_Point==Yes&&Have_right_Point==Yes&&Have_left_Point==Yes)
			Point_Status_E=Four_Top_Bottom_Letf_Right;
	else Point_Status_E=Point_Status_Error;
		switch(Point_Status_E)
	{

			case Four_Top_Bottom_Letf_Right:
			{
			if(Point_t.Locate[back].point_cnt-Point_t.Locate[front].point_cnt>10)
				Point_t.mid_x=Point_t.Locate[front].x;
			else if(Point_t.Locate[front].point_cnt-Point_t.Locate[back].point_cnt>10)
				Point_t.mid_x=Point_t.Locate[back].x;
			else
			Point_t.mid_x=(Point_t.Locate[front].x+Point_t.Locate[back].x)/2;

			Point_t.mid_y=(Point_t.Locate[left].y+Point_t.Locate[right].y)/2;
		}
		break;
	}
}


void Deal_Two_Point()
{
	if(Have_front_Point==Yes&&Have_back_Point==Yes)
		Point_Status_E=Two_Top_Bottom;
	else if(Have_front_Point==Yes&&Have_left_Point==Yes)
	  Point_Status_E=Two_Top_Left;
	else if(Have_front_Point==Yes&&Have_right_Point==Yes)
		Point_Status_E=Two_Top_Right;
	else if(Have_right_Point==Yes&&Have_left_Point==Yes)
		Point_Status_E=Two_Left_Right;
	else if(Have_right_Point==Yes&&Have_back_Point==Yes)
		Point_Status_E=Two_Bottom_Right;
	else if(Have_back_Point==Yes&&Have_left_Point==Yes)
		Point_Status_E=Two_Bottom_Left;
	else
		Point_Status_E=Point_Status_Error;


		switch(Point_Status_E)
	{
			case Two_Top_Bottom:
		{
				Point_t.mid_x=(Point_t.Locate[front].x+Point_t.Locate[back].x)/2;
				Point_t.mid_y=CAMERA_Fit_H/2;
			  EN_Offest_Rotate=Yes;
		}
		break;

		case Two_Top_Left:
		{
			Point_t.mid_x=Point_t.Locate[front].x;
			Point_t.mid_y=Point_t.Locate[left].y;
			if(Mode_One_St_Act==Mode_One_St_Act_Right_Move)//MODE 1
				Feature_Point_E=Top_Left_RightAngle;
/*			if(Mode_Two_St_Act==Mode_Two_St_Act_Right_Move)//MODE 2
				Feature_Point_E=Top_Left_RightAngle;*/
		}
		break;
			case Two_Top_Right:
		{
			Point_t.mid_x=Point_t.Locate[front].x;
			Point_t.mid_y=Point_t.Locate[right].y;
			if(Mode_One_St_Act==Mode_One_St_Act_Back_Move)
				Feature_Point_E=Top_Right_RightAngle;
		}
		break;

		case Two_Left_Right:
		{
			Point_t.mid_x=CAMERA_Fit_W/2;
			Point_t.mid_y=(Point_t.Locate[left].y+Point_t.Locate[right].y)/2;
			EN_Offest_Rotate=Yes;
		}
		break;
		case Two_Bottom_Left:
		{
			Point_t.mid_x=Point_t.Locate[back].x;
			Point_t.mid_y=Point_t.Locate[left].y;
			if(Mode_One_St_Act==Mode_One_St_Act_Front_Move)
				Feature_Point_E=Bottom_Left_RightAngle;
//			if(Mode_Two_St_Act==Mode_Two_St_Act_TrackingLine)
//				Feature_Point_E=Bottom_Left_RightAngle;
		}
		break;
		case Two_Bottom_Right:
		{
			Point_t.mid_x=Point_t.Locate[back].x;
			Point_t.mid_y=Point_t.Locate[right].y;
			if(Mode_One_St_Act==Mode_One_St_Act_Left_Second_Move)
				Feature_Point_E=Bottom_Right_RightAngle;
		}
		break;
		case Point_Status_Error:
		{
			Point_t.mid_x=CAMERA_Fit_W/2;
			Point_t.mid_y=CAMERA_Fit_H/2;
			Point_t.Rotate_Offest=0;
		}
	}

}

void Deal_Three_Point(void)
{
	#define Num  15
	if(Have_front_Point==Yes&&Have_back_Point==Yes&&Have_left_Point==Yes)
		Point_Status_E=Three_Top_Bottom_Letf;
	else if(Have_front_Point==Yes&&Have_back_Point==Yes&&Have_right_Point==Yes)
	  Point_Status_E=Three_Top_Bottom_Right;
	else if(Have_front_Point==Yes&&Have_right_Point==Yes&&Have_left_Point==Yes)
		Point_Status_E=Three_Top_Letf_Right;
	else if(Have_back_Point==Yes&&Have_right_Point==Yes&&Have_left_Point==Yes)
		Point_Status_E=Three_Bottom_Letf_Right;
	else Point_Status_E=Point_Status_Error;
	switch(Point_Status_E)
	{
		case Three_Top_Bottom_Letf:
		{
			if(Point_t.Locate[back].point_cnt<Num&&Point_t.Locate[left].point_cnt<Num
							&&Point_t.Locate[front].point_cnt<Num)//Èç¹ûÃ¿¸ö±ß½çºÚµãµÄ³¤¶È·ûºÏÐ¡·¶Î§
						{
							Point_t.mid_x=(Point_t.Locate[front].x+Point_t.Locate[back].x)/2;//ÓÐµãÎÊÌâ
							Point_t.mid_y=Point_t.Locate[left].y;
						}
			else if(Point_t.Locate[back].point_cnt>Num&&Point_t.Locate[left].point_cnt<Num
							&&Point_t.Locate[front].point_cnt<Num)//Èç¹ûÏÂ±ß½çÌ«³¤
						{
							Point_t.mid_x=Point_t.Locate[front].x;
							Point_t.mid_y=CAMERA_Fit_H-10;
						}
			else if(Point_t.Locate[back].point_cnt<Num&&Point_t.Locate[left].point_cnt<Num
							&&Point_t.Locate[front].point_cnt>Num)//Èç¹ûÉÏ±ß½çÌ«³¤
						{
							Point_t.mid_x=Point_t.Locate[back].x;
							Point_t.mid_y=10;
						}
			else	if(Point_t.Locate[back].point_cnt<Num&&Point_t.Locate[left].point_cnt>Num
							&&Point_t.Locate[front].point_cnt<Num)//Èç¹û×ó±ß½çÌ«³¤
			{
					Point_t.mid_x=10;//¸Ä
					Point_t.mid_y=CAMERA_Fit_H/2;
			}
			else	if(Point_t.Locate[back].point_cnt>Num&&Point_t.Locate[left].point_cnt>Num
							&&Point_t.Locate[front].point_cnt<Num)//Èç¹û×ó±ß½çºÍÏÂ±ß½çÌ«³¤
			{
					Point_t.mid_x=10;
					Point_t.mid_y=CAMERA_Fit_H-10;
			}

		}
		break;

		case Three_Top_Bottom_Right:
		{
			if(Point_t.Locate[back].point_cnt<Num&&Point_t.Locate[right].point_cnt<Num
							&&Point_t.Locate[front].point_cnt<Num)//Èç¹ûÃ¿¸ö±ß½çºÚµãµÄ³¤¶È·ûºÏÐ¡·¶Î§
						{
							Point_t.mid_x=(Point_t.Locate[front].x+Point_t.Locate[back].x)/2;
							Point_t.mid_y=Point_t.Locate[right].y;;
						}
			else if(Point_t.Locate[back].point_cnt>Num&&Point_t.Locate[right].point_cnt<Num
							&&Point_t.Locate[front].point_cnt<Num)//Èç¹ûÏÂ±ß½çÌ«³¤
						{
							Point_t.mid_x=Point_t.Locate[front].x;
							Point_t.mid_y=CAMERA_Fit_H-10;
						}
			else if(Point_t.Locate[back].point_cnt<Num&&Point_t.Locate[right].point_cnt<Num
							&&Point_t.Locate[front].point_cnt>Num)//Èç¹ûÉÏ±ß½çÌ«³¤
						{
							Point_t.mid_x=Point_t.Locate[back].x;
							Point_t.mid_y=10;
						}
			else	if(Point_t.Locate[back].point_cnt<Num&&Point_t.Locate[right].point_cnt>Num
							&&Point_t.Locate[front].point_cnt<Num)//Èç¹ûÓÒ±ß½çÌ«³¤
			{
					Point_t.mid_x=CAMERA_Fit_W-10;
					Point_t.mid_y=CAMERA_Fit_H/2;

			}
			else
			{

			}
		}
		break;

		case Three_Top_Letf_Right:
		{

			if(Point_t.Locate[left].point_cnt<Num&&Point_t.Locate[right].point_cnt<Num
							&&Point_t.Locate[front].point_cnt<Num)//Èç¹ûÃ¿¸ö±ß½çºÚµãµÄ³¤¶È·ûºÏÐ¡·¶Î§
						{
							Point_t.mid_x= Point_t.Locate[front].x;
							Point_t.mid_y=(Point_t.Locate[left].y+Point_t.Locate[right].y)/2;
							if(Mode_One_St_Act==Mode_One_St_Act_Right_Move_To_Circle)
											Feature_Point_E=Top_Left_Right_Circle;

						}
			else if(Point_t.Locate[left].point_cnt>Num&&Point_t.Locate[right].point_cnt<Num
							&&Point_t.Locate[front].point_cnt<Num)//Èç¹û×ó±ß½ç³¤¶È¹ý³¤
						{
							Point_t.Locate[left].y=Point_t.Locate[right].y;
							Point_t.mid_x= Point_t.Locate[front].x;
							Point_t.mid_y=(Point_t.Locate[left].y+Point_t.Locate[right].y)/2;
						}

			else	if(Point_t.Locate[left].point_cnt<Num&&Point_t.Locate[right].point_cnt>Num
							&&Point_t.Locate[front].point_cnt<Num)//Èç¹ûÓÒ±ß½ç³¤¶È¹ý³¤
						{
							Point_t.Locate[right].y=Point_t.Locate[left].y;
							Point_t.mid_x= Point_t.Locate[front].x;
							Point_t.mid_y=(Point_t.Locate[left].y+Point_t.Locate[right].y)/2;
						}
			else if(Point_t.Locate[left].point_cnt<Num&&Point_t.Locate[right].point_cnt<Num
							&&Point_t.Locate[front].point_cnt>Num)//Èç¹ûÉÏ±ß½ç³¤¶È¹ý³¤
						{
								Point_t.mid_x=CAMERA_Fit_W/2;
								Point_t.mid_y=10;
						}
			else
			{
							Point_t.mid_x= Point_t.Locate[front].x;
							Point_t.mid_y=(Point_t.Locate[left].y+Point_t.Locate[right].y)/2;
			}

		}
		break;

		case Three_Bottom_Letf_Right:
		{

			if(Point_t.Locate[left].point_cnt<Num&&Point_t.Locate[right].point_cnt<Num
							&&Point_t.Locate[back].point_cnt<Num)//Èç¹ûÃ¿¸ö±ß½çºÚµãµÄ³¤¶È·ûºÏÐ¡·¶Î§
						{
							Point_t.mid_x= Point_t.Locate[back].x;
							Point_t.mid_y=(Point_t.Locate[left].y+Point_t.Locate[right].y)/2;
							if(Mode_One_St_Act==Mode_One_St_Act_Left_First_Move&&Point_t.Locate[right].point_cnt<15)
											Feature_Point_E=Bottom_Left_Right_Circle;

						}
			else if(Point_t.Locate[left].point_cnt>Num&&Point_t.Locate[right].point_cnt<Num
							&&Point_t.Locate[back].point_cnt<Num)//Èç¹û×ó±ß½ç³¤¶È¹ý³¤
						{
							Point_t.Locate[left].y=Point_t.Locate[right].y;
							Point_t.mid_x= Point_t.Locate[back].x;
							Point_t.mid_y=(Point_t.Locate[left].y+Point_t.Locate[right].y)/2;
						}

			else	if(Point_t.Locate[left].point_cnt<Num&&Point_t.Locate[right].point_cnt>Num
							&&Point_t.Locate[back].point_cnt<Num)//Èç¹ûÓÒ±ß½ç³¤¶È¹ý³¤
						{
							Point_t.Locate[right].y=Point_t.Locate[left].y;
							Point_t.mid_x= Point_t.Locate[back].x;
							Point_t.mid_y=(Point_t.Locate[left].y+Point_t.Locate[right].y)/2;
						}
			else  if(Point_t.Locate[left].point_cnt<Num&&Point_t.Locate[right].point_cnt<Num
							&&Point_t.Locate[back].point_cnt>Num)//Èç¹ûÏÂ±ß½ç³¤¶È¹ý³¤
						{
							Point_t.mid_x=CAMERA_Fit_W/2;
							Point_t.mid_y=CAMERA_Fit_H-10;
						}
			else
			{
							Point_t.mid_x= Point_t.Locate[back].x;
							Point_t.mid_y=(Point_t.Locate[left].y+Point_t.Locate[right].y)/2;
			}

		}
		break;

		case Point_Status_Error:
		{

		}
		break;
	}

}

#define NUM 5
void Search_Line_Point(u8 HEIGHT,Position_Enum Pos)
{
		u8 i=0;
		u8 Line_Begin[NUM]={0};//5ÐÐºÚÏßÆðµã×ø±ê
		u8 Line_End[NUM]={0};//5ÐÐºÚÏß½áÊø×ø±ê
		u8 Find_OK=0,vaild_num=NUM,vaild_begin=NUM,vaild_end=NUM;
		uint16_t sum=0,Aver_Sum=0;
		uint16_t sum_begin=0,Aver_begin=0;
		uint16_t sum_end=0,Aver_end=0;

		if(HEIGHT>CAMERA_Fit_H-7)//Èç¹ûÒª¼ì²éµÄÐÐ½Ó½üµ×²¿ÔòÉÏÒÆ
			HEIGHT=CAMERA_Fit_H-7;

		for(i=0;i<NUM;i++)
		{
			Line_End[i]=CAMERA_Fit_W-1;//ÐÐ³õÊ¼½áÊøÎ»ÖÃÎª×îºóÒ»µã
			if(Find_OK==0)//Èç¹ûÈ«ÐÐÉ¨Ãè
			{
					Find_OK=Scan_One_Line(HEIGHT+i,Line_Begin,Line_End,i,Pos);
			}
			else if(Find_OK==1)//×·×ÙÉ¨Ãè
			{
				Find_OK=Edge_Track_Line((Line_Begin[i-1]+Line_End[i-1])/2,HEIGHT+i,Line_Begin,Line_End,i,Pos);
			}

			if(Line_End[i]!=0)//Èç¹û±¾ÐÐÓÐµã
			{
				sum+=(Line_Begin[i]+Line_End[i])/2;
				sum_begin+=Line_Begin[i];
				sum_end+=Line_End[i];
			}
			else //±¾ÐÐ
			{
				vaild_num--;//ÎÞÐ§µÄµã
				vaild_begin--;
				vaild_end--;
			}

			if(i==0&&Point_t.Locate[Pos].point_cnt<20 && Line_Begin[0]==0
				&&	Line_End[0]==0)//µÚÒ»µÚ2ÐÐ¶¼Ã»ÓÐµã£¬ÔòÈÏÎª±¾±ßÔµÎÞµã
			{
					if(Pos==front)
						Have_front_Point=No;
					else if(Pos==back)
						Have_back_Point=No;
					return ;//±ßÔµÎÞµã£¬²»ÔÚÑ°ÕÒ
			}
		}
		Aver_Sum=sum/vaild_num;//Çó¾ùÖµ
		Aver_begin=sum_begin/vaild_begin;
		Aver_end=sum_end/vaild_end;
		for(i=0;i<NUM;i++)
		{
			if(ABS(((Line_Begin[i]+Line_End[i])/2)-Aver_Sum)>7)//ºÍ¾ùÖµÆ«Àë¹ý´ó
			{
				sum-=(Line_Begin[i]+Line_End[i])/2;//¼õÈ¥Õâ¸öÖµ
				vaild_num--;
			}
			if(ABS((Line_Begin[i]-Aver_begin)>7))
			{
				sum_begin-=Line_Begin[i];
				vaild_begin--;
			}
			if(ABS((Line_End[i]-Aver_end)>7))
			{
				sum_end-=(Line_Begin[i]+Line_End[i])/2;
				vaild_end--;
			}
		}
		if(vaild_num!=0)
			Point_t.Locate[Pos].x=sum/vaild_num;	//×ø±ê¼ÇÂ¼
		if(vaild_begin!=0)
			Point_t.Locate[Pos].x_begin=sum_begin/vaild_begin;
		if(vaild_end!=0)
			Point_t.Locate[Pos].x_end=sum_end/vaild_end;
		/*******ÅÐ¶Ï±ßÔµµãµÄ´æÔÚ********/
		if(Point_t.Locate[Pos].x_end>Point_t.Locate[Pos].x_begin)
		{
			Point_t.Locate[Pos].x=(Point_t.Locate[Pos].x_end+Point_t.Locate[Pos].x_begin)/2;
			Point_t.Locate[Pos].y=HEIGHT+2;

			if(Pos==front)
				Have_front_Point=Yes;
			else if(Pos==back)
				Have_back_Point=Yes;

			Point_Num_E++;
		}
		else
		{
			if(Pos==front)
				Have_front_Point=Err;
			else if(Pos==back)
				Have_back_Point=Err;
		}
}


u8 Scan_One_Line(u8 HEIGHT,u8* Line_Begin,u8* Line_End,u8 i,Position_Enum Pos)
{
	u8 width;
	u8 length=0;
	for(width=0;width<CAMERA_Fit_W;width++)
	{
		if(Image_Bin[HEIGHT][width]<20)//¼ÇÂ¼±¾ÐÐºÚÏßµÄ³¤¶È
			length++;
		if(Image_Bin[HEIGHT][width]<20&&i==0)//¼ÇÂ¼µÚÒ»ÐÐºÚÏßµÄ³¤¶È
			Point_t.Locate[Pos].point_cnt++;

		if(width>0 && Image_Bin[HEIGHT][width]-Image_Bin[HEIGHT][width-1]<-200)//Èç¹û²úÉúÒ»¸öÓÉ¸ßµ½µÍµÄÌø±ä£¨°×µ½ºÚ£©
		{
			if((width>CAMERA_Fit_W-5) || (width<=(CAMERA_Fit_W-5) && Image_Bin[HEIGHT][width+1]<20
			&& Image_Bin[HEIGHT][width+2]<20))        //Èç¹ûÕâ¸öÌø±äµãºóÃæ¼¸¸öµã¶¼ÊÇºÚµÄ£¬ÔòÖ¤Ã÷ÕâÊÇÕæµÄÌø±äµã
			{
				Line_Begin[i]=width;//
			}
		}
		else if(width>0 && Image_Bin[HEIGHT][width]-Image_Bin[HEIGHT][width-1]>200)//Èç¹û²úÉúÒ»¸öÓÉ¸ßµ½µÍµÄÌø±ä
		{
			if((width>CAMERA_Fit_W-5) || (width<=(CAMERA_Fit_W-5)&&Image_Bin[HEIGHT][width+1]>200
			&&Image_Bin[HEIGHT][width+2]>200 && length>3))//Èç¹ûÕâ¸öÌø±äµãºóÃæ¼¸¸öµã¶¼ÊÇ°×µÄ£¬ÔòÖ¤Ã÷ÕâÊÇÕæµÄÌø±äµã
			{
				Line_End[i]=width;
			}
		}
	}

	if(Line_Begin[i]<=5&&Line_End[i]>=(CAMERA_Fit_W-5)&&length<10)//Ã»ÓÐ¼ì²âµ½Ìø±äÑØ£¬Í¬Ê±ÅÅ³ýÈ«ºÚµÄÇé¿ö
	{
		Line_Begin[i]=0;//±¾ÐÐÎÞµã
		Line_End[i]=0;
		return 0;
	}
	else if(Line_Begin[i]<=5&&Line_End[i]>=(CAMERA_Fit_W-5)&&length>CAMERA_Fit_W-20)//Ã»ÓÐ¼ì²âµ½Ìø±äÑØµ«±¾ÐÐÈ«ºÚ
		return 0;//ÏÂÒ»ÐÐÈ«ÐÐÉ¨Ãè
	else if(ABS(Line_Begin[i]-Line_End[i])<20&&length<20)//ºÚÏß³¤¶ÈÔÚºÏÀí·¶Î§ÄÚ
		return 1;//ÏÂÒ»ÐÐ¿ªÊ¼±ßÔµ×·×Ù
	else return 0;
}

/*******************************************************************************
  * @º¯ÊýÃû³Æ   Edge_Track_Line(u8 x,u8 HEIGHT,u8* Line_Begin,u8* Line_End,u8 i,Position_Enum Pos)
  * @º¯ÊýËµÃ÷   ±ßÑØ×·×Ù£¨ÐÐ£©
  * @ÊäÈë²ÎÊý   x:ÉÏÒ»ÐÐºÚÏßÖÐµã×ø±ê
								HEIGHT:±¾ÐÐ¸ß¶È
								Line_Begin:ºÚÏßÆðÊ¼×ø±ê´æ´¢
								Line_End:ºÚÏß½áÊø×ø±ê´æ´¢
								i:Êý×éÎ»Êý
								Pos:·½Î»
	* @·µ»Ø²ÎÊý   ±¾´Î±ßÑØ×·×ÙÊÇ·ñ³É¹¦
*******************************************************************************/
u8 Edge_Track_Line(u8 x,u8 HEIGHT,u8* Line_Begin,u8* Line_End,u8 i,Position_Enum Pos)
{
	u8 begin=0;
	u8 End=0;
	u8 width;
	u8 length=0;

	if(x>CAMERA_Fit_W-20)//ÏÞÖÆ³¬³ö×î´ó·¶Î§
		End=CAMERA_Fit_W-1;
	else
		End=x+15;
	if(x<16)//ÏÞÖÆ³¬³ö×îÐ¡·¶Î§
		begin=0;
	else
		begin=x-15;

	//É¨Ãè±¾ÐÐ£¬Éè¶¨ÆðÊ¼ºÍ½áÎ²×ø±ê
	for(width=begin;width<End;width++)
	{
		if(Image_Bin[HEIGHT][width]<20)//¼ÇÂ¼±¾ÐÐºÚÏßµÄ³¤¶È
			length++;

		if(width>0&&Image_Bin[HEIGHT][width]-Image_Bin[HEIGHT][width-1]<-200)//Èç¹û²úÉúÒ»¸öÓÉ¸ßµ½µÍµÄÌø±ä£¨°×µ½ºÚ£©
		{
			if((width>CAMERA_Fit_W-5)||(width<=(CAMERA_Fit_W-5)&&Image_Bin[HEIGHT][width+1]<20&&
			Image_Bin[HEIGHT][width+2]<20))         //Èç¹ûÕâ¸öÌø±äµãºóÃæ¼¸¸öµã¶¼ÊÇºÚµÄ£¬ÔòÖ¤Ã÷ÕâÊÇÕæµÄÌø±äµã
			{
				Line_Begin[i]=width;
			}
		}
		else if(width>0&&Image_Bin[HEIGHT][width]-Image_Bin[HEIGHT][width-1]>200)//Èç¹û²úÉúÒ»¸öÓÉ¸ßµ½µÍµÄÌø±ä
		{
			if((width>CAMERA_Fit_W-5)||(width<=(CAMERA_Fit_W-5)&&Image_Bin[HEIGHT][width+1]>200&&
				Image_Bin[HEIGHT][width+2]>200&&length>3))//Èç¹ûÕâ¸öÌø±äµãºóÃæ¼¸¸öµã¶¼ÊÇ°×µÄ£¬ÔòÖ¤Ã÷ÕâÊÇÕæµÄÌø±äµã
			{
				Line_End[i]=width;
			}
		}
	}
	if(Line_Begin[i]<=5&&Line_End[i]>=CAMERA_Fit_W-5)//Ã»ÓÐ¼ì²âµ½Ìø±äÑØ
	{
		return Scan_One_Line(HEIGHT,Line_Begin,Line_End,i,Pos);//É¨ÃèÈ«ÐÐ
	}
	else
		return 1;
}



void Search_Column_Point(u8 WIDTH,Position_Enum Pos)
{
	#define NUM 5
	u8 i=0;
	u8 Column_Begin[NUM]={0};
	u8 Column_End[NUM]={0};
	u8 Find_OK=0,vaild_num=NUM,vaild_begin=NUM,vaild_end=NUM;
	uint16_t sum=0,Aver_Sum=0;
	uint16_t sum_begin=0,Aver_begin=0;
	uint16_t sum_end=0,Aver_end=0;
	if(WIDTH>CAMERA_Fit_W-7)
		WIDTH=CAMERA_Fit_W-7;

	for(i=0;i<NUM;i++)
	{
		Column_End[i]=CAMERA_Fit_H-1;
		if(Find_OK==0)//Èç¹ûÉÏÒ»ÐÐÃ»ÓÐÕÒµ½ÌØÕ÷µã
		{
				Find_OK=Scan_One_Column(WIDTH+i,Column_Begin,Column_End,i,Pos);//ÕÒµ½µã·µ»Ø1
		}
		else if(Find_OK==1)
		{
			Find_OK=Edge_Track_Column((Column_Begin[i-1]+Column_End[i-1])/2,WIDTH+i,Column_Begin,Column_End,i,Pos);
		}

		if(Column_End[i]!=0)
		{
			sum+=(Column_Begin[i]+Column_End[i])/2;
			sum_begin+=Column_Begin[i];
			sum_end+=Column_End[i];
		}
		else
		{
			vaild_num--;
			vaild_begin--;
			vaild_end--;
		}
		if(i==1&&Point_t.Locate[Pos].point_cnt<=20&&Column_Begin[0]==0&&
									Column_End[0]==0)
		{
			if(Pos==left)
				Have_left_Point=No;
			else if(Pos==right)
				Have_right_Point=No;
			return ;
		}

	}
	Aver_Sum=sum/vaild_num;
	Aver_begin=sum_begin/vaild_begin;
	Aver_end=sum_end/vaild_end;
	for(i=0;i<NUM;i++)
	{
		if(ABS(((Column_Begin[i]+Column_End[i])/2)-Aver_Sum)>7)
		{
			sum-=(Column_Begin[i]+Column_End[i])/2;
			vaild_num--;
		}

		if(ABS((Column_Begin[i]-Aver_begin)>7))
		{
			sum_begin-=Column_Begin[i];
			vaild_begin--;
		}

		if(ABS((Column_Begin[i]-Aver_end)>7))
		{
			sum_end-=(Column_Begin[i]+Column_End[i])/2;
			vaild_end--;
		}
	}
	if(vaild_num!=0)
		Point_t.Locate[Pos].y=sum/vaild_num;
	if(vaild_begin!=0)
		Point_t.Locate[Pos].y_begin=sum_begin/vaild_begin;
	if(vaild_end!=0)
		Point_t.Locate[Pos].y_end=sum_end/vaild_end;
	/*******ÅÐ¶Ï±ßÔµµãµÄ´æÔÚ********/
	if(Point_t.Locate[Pos].y_end>Point_t.Locate[Pos].y_begin)
	{
		Point_t.Locate[Pos].x=WIDTH+2;
		Point_t.Locate[Pos].y=(Point_t.Locate[Pos].y_end+Point_t.Locate[Pos].y_begin)/2;

		if(Pos==left)
				Have_left_Point=Yes;
			else if(Pos==right)
				Have_right_Point=Yes;

		Point_Num_E++;
	}
	else
	{
			if(Pos==left)
				Have_left_Point=Err;
			else if(Pos==right)
				Have_right_Point=Err;
	}
}

u8 Scan_One_Column(u8 WIDTH,u8* Column_Begin,u8* Column_End,u8 i,Position_Enum Pos)
{
	u8 height=0;
	u8 length=0;
		for(height=0;height<CAMERA_Fit_H;height++)
		{
				if(Image_Bin[height][WIDTH]<20)		//¼ÇÂ¼±¾ÁÐºÚÏß³¤¶È
					length++;
				if(Image_Bin[height][WIDTH]<20&&i==0)//¼ÇÂ¼µÚÒ»ÁÐºÚÏß³¤¶È
					Point_t.Locate[Pos].point_cnt++;

			if(height>0&&Image_Bin[height][WIDTH]-Image_Bin[height-1][WIDTH]<-200)//Èç¹û²úÉúÒ»¸öÓÉ¸ßµ½µÍµÄÌø±ä
			{
				if((height>CAMERA_Fit_H-5)||(height<=(CAMERA_Fit_H-5)&&Image_Bin[height+1][WIDTH]<20&&
									Image_Bin[height+2][WIDTH]<20))//Èç¹ûÕâ¸öÌø±äµãºóÃæ¼¸¸öµã¶¼ÊÇºÚµÄ£¬ÔòÖ¤Ã÷ÕâÊÇÕæµÄÌø±äµã
				{
					Column_Begin[i]=height;//

				}
			}
			else 	if(height>0&&Image_Bin[height][WIDTH]-Image_Bin[height-1][WIDTH]>200)
			{
				if((height>CAMERA_Fit_H-5)||((height<=(CAMERA_Fit_H-5)&&Image_Bin[height+1][WIDTH]>200&&
					Image_Bin[height+2][WIDTH]>200)&&length>3))
				{
					Column_End[i]=height;
				}
			}
		}
	if(Column_Begin[i]<=5&&Column_End[i]>=(CAMERA_Fit_H-5)&&length<10)//Ã»ÓÐ¼ì²âµ½Ìø±äÑØ£¬Í¬Ê±ÅÅ³ýÈ«ºÚµÄÇé¿ö
	{
		Column_Begin[i]=0;
		Column_End[i]=0;
		return 0;
	}
	else if(Column_Begin[i]<=5&&Column_End[i]>=(CAMERA_Fit_H-5)&&length>CAMERA_Fit_H-20)//Ã»ÓÐ¼ì²âµ½Ìø±äÑØµ«±¾ÁÐÈ«ºÚ
		return 0;
	else if(ABS(Column_End[i]-Column_Begin[i])<15&&length<15)
		return 1;
	else return 0;
}

u8 Edge_Track_Column(u8 y,u8 WIDTH,u8* Column_Begin,u8* Column_End,u8 i,Position_Enum Pos)
{
	u8 begin=0;
	u8 End=0;
	u8 height;
	u8 length=0;

	if(y>CAMERA_Fit_H-20)
		End=CAMERA_Fit_H-1;
	else End=y+15;
	if(y<16)
		begin=0;
	else begin=y-15;
		for(height=begin;height<End;height++)
	{
		if(Image_Bin[height][WIDTH]<20)
			length++;
		if(height>0&&Image_Bin[height][WIDTH]-Image_Bin[height-1][WIDTH]<-200)
			{
				if((height>CAMERA_Fit_H-5)||(height<=(CAMERA_Fit_H-5)&&Image_Bin[height+1][WIDTH]<20&&
									Image_Bin[height+2][WIDTH]<20))
				{
					Column_Begin[i]=height;

				}
			}
			else 	if(height>0&&Image_Bin[height][WIDTH]-Image_Bin[height-1][WIDTH]>200)
			{
				if((height>CAMERA_Fit_H-5)||(height<=(CAMERA_Fit_H-5)&&Image_Bin[height+1][WIDTH]>200&&
					Image_Bin[height+2][WIDTH]>200&&length>3))
				{
					Column_End[i]=height;
				}
			}
	}


	if(Column_Begin[i]<=5&&Column_End[i]<=CAMERA_Fit_H-5)
	{
		return Scan_One_Column(WIDTH,Column_Begin,Column_End,i,Pos);
	}
	else
		return 1;
}

